//
// Created by wyh on 24-4-17.
//
#include <iostream>
#include <fstream>
#include <unistd.h>
#include <opencv2/opencv.hpp>
#include "model.h"
#include "utils.h"
#include "preprocess.h"
#include "postprocess.h"
#include "cuda_utils.h"
#include "logging.h"

#include "YoloDetector.h"
#include "Tracking.h"
#include "BYTETracker.h"

Logger gLogger;
using namespace nvinfer1;
using namespace std;
using namespace cv;
const int kOutputSize = kMaxNumOutputBbox * sizeof(Detection) / sizeof(float) + 1;
const static int kOutputSegSize = 32 * (kInputH / 4) * (kInputW / 4);
namespace ORB_SLAM3
{

static cv::Rect get_downscale_rect(float bbox[4], float scale) {

  float left = bbox[0];
  float top  = bbox[1];
  float right  = bbox[0] + bbox[2];
  float bottom = bbox[1] + bbox[3];

  left    = left < 0 ? 0 : left;
  top     = top < 0 ? 0: top;
  right   = right > 640 ? 640 : right;
  bottom  = bottom > 640 ? 640: bottom;

  left   /= scale;
  top    /= scale;
  right  /= scale;
  bottom /= scale;
  return cv::Rect(int(left), int(top), int(right - left), int(bottom - top));
}

cv::Mat scale_mask(cv::Mat mask, cv::Mat& img) {
    int x, y, w, h;
    float r_w = kInputW / (img.cols * 1.0);
    float r_h = kInputH / (img.rows * 1.0);
    if (r_h > r_w) {
        w = kInputW;
        h = r_w * img.rows;
        x = 0;
        y = (kInputH - h) / 2;
    } else {
        w = r_h * img.cols;
        h = kInputH;
        x = (kInputW - w) / 2;
        y = 0;
    }
    cv::Rect r(x, y, w, h);
    cv::Mat res;
    cv::resize(mask(r), res, img.size());
    return res;
}

std::vector<cv::Mat> process_mask(const float* proto, int proto_size, std::vector<Detection>& dets,cv::Mat& img) {

  std::vector<cv::Mat> masks;
  for (size_t i = 0; i < dets.size(); i++) {

    cv::Mat mask_mat = cv::Mat::zeros(kInputH / 4, kInputW / 4, CV_32FC1);
    auto r = get_downscale_rect(dets[i].bbox, 4);

    for (int x = r.x; x < r.x + r.width; x++) {
      for (int y = r.y; y < r.y + r.height; y++) {
        float e = 0.0f;
        for (int j = 0; j < 32; j++) {
            e += dets[i].mask[j] * proto[j * proto_size / 32 + y * mask_mat.cols + x];
        }
        e = 1.0f / (1.0f + expf(-e));
        mask_mat.at<float>(y, x) = e;
      }
    }
    cv::resize(mask_mat, mask_mat, cv::Size(kInputW, kInputH));
    cv::Mat img_mask = scale_mask(mask_mat, img);
    masks.push_back(img_mask);
  }
  return masks;
}




void deserialize_engine(std::string &engine_name, IRuntime **runtime, ICudaEngine **engine, IExecutionContext **context)
{
    std::ifstream file(engine_name, std::ios::binary);
    if (!file.good())
    {
        std::cerr << "read " << engine_name << " error!" << std::endl;
        assert(false);
    }
    size_t size = 0;
    file.seekg(0, file.end);
    size = file.tellg();
    file.seekg(0, file.beg);
    char *serialized_engine = new char[size];
    assert(serialized_engine);
    file.read(serialized_engine, size);
    file.close();

    *runtime = createInferRuntime(gLogger);
    assert(*runtime);
    *engine = (*runtime)->deserializeCudaEngine(serialized_engine, size);
    assert(*engine);
    *context = (*engine)->createExecutionContext();
    assert(*context);
    delete[] serialized_engine;
}

void prepare_buffer(ICudaEngine *engine, float **input_buffer_device, float **output_buffer_device, float **output_seg_buffer_device,
                    float **output_buffer_host,float **output_seg_buffer_host ,float **decode_ptr_host, float **decode_ptr_device, std::string cuda_post_process) {
    assert(engine->getNbBindings() == 3);
    // In order to bind the buffers, we need to know the names of the input and output tensors.
    // Note that indices are guaranteed to be less than IEngine::getNbBindings()
    const int inputIndex = engine->getBindingIndex(kInputTensorName);
    const int outputIndex = engine->getBindingIndex(kOutputTensorName);
    const int outputIndex_seg = engine->getBindingIndex("proto");

    assert(inputIndex == 0);
    assert(outputIndex == 1);
    assert(outputIndex_seg == 2);
    // Create GPU buffers on device
    CUDA_CHECK(cudaMalloc((void **) input_buffer_device, kBatchSize * 3 * kInputH * kInputW * sizeof(float)));
    CUDA_CHECK(cudaMalloc((void **) output_buffer_device, kBatchSize * kOutputSize * sizeof(float)));
    CUDA_CHECK(cudaMalloc((void **) output_seg_buffer_device, kBatchSize * kOutputSegSize * sizeof(float)));
    
    if (cuda_post_process == "c") {
        *output_buffer_host = new float[kBatchSize * kOutputSize];
        *output_seg_buffer_host = new float[kBatchSize * kOutputSegSize];
    } else if (cuda_post_process == "g") {
        if (kBatchSize > 1) {
            std::cerr << "Do not yet support GPU post processing for multiple batches" << std::endl;
            exit(0);
        }
        // Allocate memory for decode_ptr_host and copy to device
        *decode_ptr_host = new float[1 + kMaxNumOutputBbox * bbox_element];
        CUDA_CHECK(cudaMalloc((void **)decode_ptr_device, sizeof(float) * (1 + kMaxNumOutputBbox * bbox_element)));
    }
}

void infer(IExecutionContext &context, cudaStream_t &stream, void **buffers, float *output, float *output_seg,int batchsize, float* decode_ptr_host, float* decode_ptr_device, int model_bboxes, std::string cuda_post_process) {
    // infer on the batch asynchronously, and DMA output back to host
    auto start = std::chrono::system_clock::now();
    context.enqueue(batchsize, buffers, stream, nullptr);
    if (cuda_post_process == "c") {

        std::cout << "kOutputSize:" << kOutputSize <<std::endl;
        CUDA_CHECK(cudaMemcpyAsync(output, buffers[1], batchsize * kOutputSize * sizeof(float), cudaMemcpyDeviceToHost,stream));
        std::cout << "kOutputSegSize:" << kOutputSegSize <<std::endl;
        CUDA_CHECK(cudaMemcpyAsync(output_seg, buffers[2], batchsize * kOutputSegSize * sizeof(float), cudaMemcpyDeviceToHost, stream));

        auto end = std::chrono::system_clock::now();
        std::cout << "inference time: " << std::chrono::duration_cast<std::chrono::milliseconds>(end - start).count() << "ms" << std::endl;
    } else if (cuda_post_process == "g") {
        CUDA_CHECK(cudaMemsetAsync(decode_ptr_device, 0, sizeof(float) * (1 + kMaxNumOutputBbox * bbox_element), stream));
        cuda_decode((float *)buffers[1], model_bboxes, kConfThresh, decode_ptr_device, kMaxNumOutputBbox, stream);
        cuda_nms(decode_ptr_device, kNmsThresh, kMaxNumOutputBbox, stream);//cuda nms
        CUDA_CHECK(cudaMemcpyAsync(decode_ptr_host, decode_ptr_device, sizeof(float) * (1 + kMaxNumOutputBbox * bbox_element), cudaMemcpyDeviceToHost, stream));
        auto end = std::chrono::system_clock::now();
        std::cout << "inference and gpu postprocess time: " << std::chrono::duration_cast<std::chrono::milliseconds>(end - start).count() << "ms" << std::endl;
    }

    CUDA_CHECK(cudaStreamSynchronize(stream));
}
bool YoloDetector::isTrackingClass(int class_id){
    for (auto& c : this->trackClasses){
        if (class_id == c) return true;
    }
    return false;
};
YoloDetector::YoloDetector():mbFinishRequested(false) 
{
    ptracker = new BYTETracker(30,30);
//    read_labels(labels_filename, labels_map);
//    assert(kNumClass == labels_map.size());
    std::ifstream f(labels_filename);
    std::string name = "";
    while (std::getline(f, name))
    {
        this->class_names.push_back(name);
    }

    // Deserialize the engine from file
    deserialize_engine(engine_name, &runtime, &engine, &context);
    CUDA_CHECK(cudaStreamCreate(&stream));
    cuda_preprocess_init(kMaxInputImageSize);
    auto out_dims = engine->getBindingDimensions(1);
    model_bboxes = out_dims.d[0];

    // Prepare cpu and gpu buffers

    prepare_buffer(engine, &device_buffers[0], &device_buffers[1], &device_buffers[2], &output_buffer_host, &output_seg_buffer_host,&decode_ptr_host, &decode_ptr_device, cuda_post_process);

}

YoloDetector::~YoloDetector(){
    // Release stream and buffers
    cudaStreamDestroy(stream);
    CUDA_CHECK(cudaFree(device_buffers[0]));
    CUDA_CHECK(cudaFree(device_buffers[1]));
    CUDA_CHECK(cudaFree(device_buffers[2]));
    CUDA_CHECK(cudaFree(decode_ptr_device));
    delete[] decode_ptr_host;
    delete[] output_buffer_host;
    delete[] output_seg_buffer_host;
    cuda_preprocess_destroy();
    // Destroy the engine
    delete context;
    delete engine;
    delete runtime;

}

void YoloDetector::DetectByTensorRT(cv::Mat image,std::vector<YoloBoundingBox> &YoloBoundingBoxlist, std::vector<cv::Rect2f>& DynamicArea,std::vector<STrack>& Strackslist){

    std::vector<cv::Mat> img_batch;
    img_batch.push_back(image);


    // Preprocess
    cuda_batch_preprocess(img_batch, device_buffers[0], kInputW, kInputH, stream);
    // Run inference
    infer(*context, stream, (void **)device_buffers, output_buffer_host, output_seg_buffer_host,kBatchSize, decode_ptr_host, decode_ptr_device, model_bboxes, cuda_post_process);
    std::vector<std::vector<Detection>> res_batch;

    // NMS
    batch_nms(res_batch, output_buffer_host, img_batch.size(), kOutputSize, kConfThresh, kNmsThresh);

    //第几张图
    for (size_t b = 0; b < img_batch.size(); b++) {
        auto& res = res_batch[b];
        cv::Mat img = img_batch[b];
        auto masks = process_mask(&output_seg_buffer_host[b * kOutputSegSize], kOutputSegSize, res,image);
//        draw_mask_bbox(img, res, masks, labels_map);
//        cv::imwrite("_" + img_name_batch[b], img);
        std::vector<Object> objects;
        for (int ir=0;ir<res_batch[b].size();ir++)
        {
            auto rect1 = get_rect(img_batch[0], res_batch[0][ir].bbox);
            float conf = res_batch[b][ir].conf;
            float class_id = res_batch[b][ir].class_id;
            std::string  label = class_names[res_batch[b][ir].class_id];

//            int track_id = NULL;
//            std::vector<float> track_tlwh = {};

            if (isTrackingClass(class_id)){  //、、、判断是人或其他目标  抓斗和沙子都要追
                cv::Rect_<float> rect((float)rect1.x, (float)rect1.y, (float)rect1.width, (float)rect1.height);
                Object obj {rect, (int)class_id, conf};
                objects.push_back(obj);
//                if (class_names[res_batch[b][ir].class_id] == "person") { //判断是否动态目标
                DynamicArea.push_back(rect1);
                YoloBoundingBoxlist.push_back(YoloBoundingBox(rect1, class_id, conf,label,masks[ir]));
            }
        }
        // track
        std::vector<STrack> output_stracks = ptracker->update(objects);
        for (int i = 0; i < output_stracks.size(); i++)
        {
            std::vector<float> tlwh = output_stracks[i].tlwh;
            if (tlwh[2] * tlwh[3] > 20)
            {
                Strackslist.push_back(output_stracks[i]);
            }
        }

//         // 这张图的第几个目标
//         for(int ir=0;ir<res_batch[b].size();ir++) {
//             auto rect1 = get_rect(img_batch[0], res_batch[0][ir].bbox);
//             if (class_names[res_batch[b][ir].class_id] == "person") {
//                 DynamicArea.push_back(rect1);
// //                yoloBoundingBoxList.push_back(YoloBoundingBox(rect1, class_names[res_batch[0][ir].class_id], res_batch[0][ir].conf));
                // yoloBoundingBoxList.push_back(YoloBoundingBox(rect1, class_names[res_batch[b][ir].class_id], res_batch[b][ir].conf,masks[ir]));
//                 // 打印矩形的左上角坐标和宽度、高度在一行上
//                  std::cout << "--------Rect1 (x, y, width, height): (" << rect1.x << ", " << rect1.y << ", "<< rect1.width << ", " << rect1.height << ")" << std::endl;
//                  std::cout << "--------Class: " << class_names[res_batch[0][ir].class_id] << " Confidence: " << res_batch[0][ir].conf << std::endl;

//             }
//         }
    }


}

void YoloDetector::Run() {
    while (1)
    {
//        usleep(1);
        if(!isNewImgArrived())
            continue; //如果没有新图像到达，则使用 continue 关键字跳过当前循环，继续执行下一次循环。
//        if(mImg.channels() == 1)
//            cvtColor(mImg,mImg,cv::COLOR_GRAY2RGB);
//        std::vector<YoloBoundingBox> yoloBoundingBoxList;
        std::vector<YoloBoundingBox> yoloBoundingBoxList;
        vector<cv::Rect2f> DynamicArea;
        std::vector<STrack> Strackslist;
//        std::vector<cv::Mat> masks;
        if(mbYOLO)
        // if(true)
        {
#ifdef TIMES
            std::chrono::steady_clock::time_point yolo1 = std::chrono::steady_clock::now();
#endif
//                 DetectByTensorRT(mImg, mpTracker->mCurrentFrame.yoloBoundingBoxList);
                DetectByTensorRT(mImg, yoloBoundingBoxList, DynamicArea, Strackslist);
           mpTracker->yoloBoundingBoxList = yoloBoundingBoxList;
           mpTracker->mvDynamicArea = DynamicArea;
           mpTracker->mvStrackslist = Strackslist;
#ifdef TIMES
            std::chrono::steady_clock::time_point yolo2 = std::chrono::steady_clock::now();
            cout << "YOLO time:" << std::chrono::duration_cast<std::chrono::duration<double> >(yolo2 - yolo1).count() <<endl;
#endif
        }
        mpTracker->mbNewSegImgFlag=true;
        if(CheckFinish())
           break;
    }
}

void YoloDetector::SetTracker(Tracking* pTracker) {
    mpTracker = pTracker;
}

bool YoloDetector::isNewImgArrived() {
    unique_lock<mutex> lock(mMutexGetNewImg);
    if(mbNewImgFlag) {
        mbNewImgFlag = false;
        return true;
    }
    else
        return false;
}

bool YoloDetector::CheckFinish()
{
    unique_lock<mutex> lock(mMutexFinish);
    return mbFinishRequested;
}

void YoloDetector::RequestFinish()
{
    unique_lock<mutex> lock(mMutexFinish);
    mbFinishRequested=true;
}

YoloBoundingBox::YoloBoundingBox(cv::Rect2f input_rect, int class_id, float score, std::string input_label, cv::Mat mask){
    this->rect = input_rect;
    this->class_id = class_id,
    this->label = input_label;
    this->score = score;
    this->width = input_rect.br().x-input_rect.tl().x;
    this->height = input_rect.br().y-input_rect.tl().x;
    this->mask = mask;
}


}